//*************************************************************************
//	Includes Section
//*************************************************************************
#include "PlateLocalization.h"
#include "LPR.h"
#include "cv.h"
#include "highgui.h"
#include "cxtypes.h"
#include "DebugImage.h"
#include "CarDB.h"


#include <iostream>
using std::cout;
using std::endl;

#include <queue>
using std::queue;

//	Yellow HSV properties
#define YELLOW_H_MIN 15
#define YELLOW_H_MAX 40
#define YELLOW_S_MIN 100
#define YELLOW_S_MAX 500
#define YELLOW_V_MIN 0
#define YELLOW_V_MAX 200

#define MARGIN 0

#define YELLOW_RGB_FILTERING 0	//	No RGB filtering - Set to 0

#define PLATERES 20

//**************************************************************************
//	Public methods implementations
//**************************************************************************
//--------------------------------------------------------------------------
//	Function:		Instance()
//	Description:	Singleton instance retriver.
//--------------------------------------------------------------------------
PlateLocalization* PlateLocalization::Instance()
{
	static PlateLocalization plInst;
	return &plInst;
}

//--------------------------------------------------------------------------
//	Function:		FindLicenseNember()
//	Description:	Public method, that finding the license plate number of
//					a given picture or a given picture vector. 
//					It releases the img resources after finishing processing 
//					the data.
//--------------------------------------------------------------------------
void PlateLocalization::FindLicenseNumber(IplImage *img)
{

	//cvCvtColor(img,img,CV_BGR2HSV);
	//DebugImage dbug(img, "test");
	//	TODO : Read about threads and implement InvokeFinding Asyncly
	InvokeFinding(img);
}

void PlateLocalization::FindLicenseNumber(ImageVec vec)
{
	//	TODO : Implement LPN algorithm to a series of images given in a vector
}



//**************************************************************************
//	Private methods implementations
//**************************************************************************
//--------------------------------------------------------------------------
//	Function:		InvokeFinding()
//	Description:	A new working thread "entry point". The whole LPN algo'
//					is implemented in this method.
//--------------------------------------------------------------------------
void PlateLocalization::InvokeFinding(IplImage *img)
{
	//	-----------------------------------------
	//	Finding Algorithm :	
	//	-------------------
	//	1.	Notify the db of a new car entry suspected.		-	No need?
	//	2.	Yellofy the image and locate suitable LP yellow object.
	//	3.	Find squares in yellow remaining object.
	//	4.	Rotate and trim the LP.
	//	5.	Send Trimmed img to OCR process.
	//	6.	Notify the db of the car LPN.
	//	*.	Upon a step failure, notify the db.		-	No need?
	//	-----------------------------------------


	IplImage *src = cvCloneImage(img);

	//	1.	Notify the db of a new car entry suspected.
	//	TODO: use db interface to notify a new entry and give a sn
	//	NotifyDB(NOTIFY_CAR_ENTRY, sn);


	//	2.	Yellofy the image and locate suitable LP yellow object.
	if (!ExtractYellowFromImg(src))
	{	
		//	TODO: use db interface to disregard the last car entry suspicion
		//	NotifyDB(NOTIFY_CAR_ENTRY_ERROR, 0);
		cvReleaseImage(&src);
		return;
	}

	//	3.	Find squares in yellow remaining object.
	CvPoint SquarePoints[4];
	if (!FindSquare(src,SquarePoints))
	{
		//	TODO: use db interface to disregard the last car entry suspicion
		//	NotifyDB(NOTIFY_CAR_ENTRY_ERROR, 0);
		cvReleaseImage(&src);
		return;
	}

	IplImage *trimmedLP;
	cvCopyImage(img, src);
	if (!TrimPicture(src, &trimmedLP, SquarePoints)){
		cvReleaseImage(&src);
		return;
	}

	//	Flip picture from ophir
	IplImage *flippedTrimmed = cvCreateImage(cvGetSize(trimmedLP), 8, 3);

#ifdef _DEBUG
	DebugImage flippedsadioaghaoerwg(flippedTrimmed, "flipped");
#endif

	for (int x = 0; x < trimmedLP->width; ++x)
	{
		for (int y = 0; y < trimmedLP->height; ++y)
		{
			uchar b (CV_IMAGE_ELEM(trimmedLP, uchar, y, x * 3 + 0));
			uchar g (CV_IMAGE_ELEM(trimmedLP, uchar, y, x * 3 + 1));
			uchar r (CV_IMAGE_ELEM(trimmedLP, uchar, y, x * 3 + 2));

			CV_IMAGE_ELEM(flippedTrimmed, uchar, trimmedLP->height - 1 - y, x * 3 + 0) = b;
			CV_IMAGE_ELEM(flippedTrimmed, uchar, trimmedLP->height - 1 - y, x * 3 + 1) = g;
			CV_IMAGE_ELEM(flippedTrimmed, uchar, trimmedLP->height - 1 - y, x * 3 + 2) = r;
		}
	}
	cvReleaseImage(&trimmedLP);

	LPR OCR(flippedTrimmed);
	long LPN = OCR.result();

	ParkinglotDB::instance().CarEntered(sn);
	ParkinglotDB::instance().SetLicensePlate(sn, LPN);
	sn++;

	printf("%ld - Entered the parking lot.\n", LPN);



	//	Debug - draw square
#ifdef _DEBUG

	for (int x = 0; x < img->width; ++x)
	{
		for (int y = 0; y < img->height; ++y)
		{
			uchar b (CV_IMAGE_ELEM(src, uchar, y, x * 3 + 0));
			uchar g (CV_IMAGE_ELEM(src, uchar, y, x * 3 + 1));
			uchar r (CV_IMAGE_ELEM(src, uchar, y, x * 3 + 2));

			if (r != 0 || g != 0 || b != 0) {
				CV_IMAGE_ELEM(img, uchar, y, x * 3 + 0) = b;
				CV_IMAGE_ELEM(img, uchar, y, x * 3 + 1) = g;
				CV_IMAGE_ELEM(img, uchar, y, x * 3 + 2) = r;
			}
		}
	}
#endif


	//cvCopyImage(src,img);


}


//--------------------------------------------------------------------------
//---------															--------
//---------			Extracting largest yellow methods				--------
//---------															--------
//--------------------------------------------------------------------------
//	Function:		TresholdYellow()
//	Description:	Tresholding the picture's yellow objects using HSV mode
//					filtering.
//--------------------------------------------------------------------------
bool PlateLocalization::TresholdYellow(IplImage *img)
{
	//	turning the rgb image to HSV color scheme

	IplImage *tmp = cvCloneImage(img);
	cvCvtColor(tmp, tmp, CV_BGR2HSV);

	//	Deleting edges
	for (int x = 0; x < img->width; ++x)
	{
		for (int y = 0; y < img->height; ++y)
		{
			if (x < img->width * MARGIN || x > img->width * (1-MARGIN) || y < img->height * MARGIN || y > img->height * (1-MARGIN)){
				CV_IMAGE_ELEM(img, uchar, y, x * 3 + 0) = 0;
				CV_IMAGE_ELEM(img, uchar, y, x * 3 + 1) = 0;
				CV_IMAGE_ELEM(img, uchar, y, x * 3 + 2) = 0;
			}
		}
	}

	//DebugImage sdlkjfhadfh(img,"asdahldgkadshgkasjdg");
	//	Iterating through image pixels and deleting all the ones that aren't yellow
	for (int x = 0; x < img->width; ++x)
	{
		for (int y = 0; y < img->height; ++y)
		{
			uchar *h (&CV_IMAGE_ELEM(tmp, uchar, y, x * 3 + 0));
			uchar *s (&CV_IMAGE_ELEM(tmp, uchar, y, x * 3 + 1));
			uchar *v (&CV_IMAGE_ELEM(tmp, uchar, y, x * 3 + 2));
			uchar *r (&CV_IMAGE_ELEM(img, uchar, y, x * 3 + 0));
			uchar *g (&CV_IMAGE_ELEM(img, uchar, y, x * 3 + 1));
			uchar *b (&CV_IMAGE_ELEM(img, uchar, y, x * 3 + 2));

			if (	YELLOW_H_MIN < *h	&&	YELLOW_H_MAX > *h	&& 
				YELLOW_S_MIN < *s	&&	YELLOW_S_MAX > *s	&& 
				YELLOW_V_MIN < *v	&&	YELLOW_V_MAX > *v	&&
				YELLOW_RGB_FILTERING < *r &&
				YELLOW_RGB_FILTERING < *g &&
				YELLOW_RGB_FILTERING < *b						)
				continue;

			//	Delete pixel that are out of the HSV and RGB given ranges
			*r = *g = *b = 0;
		}
	}

	//DebugImage dasfasdfasdfasdf(tmp,"asdahldgkafsdfdshgkasjdg");
	cvReleaseImage(&tmp);

	return true;
}

//--------------------------------------------------------------------------
//	Function:		SegmentLargestObject()
//	Description:	Segmenting the objects in the image, and delete all the 
//					smaller ones.
//--------------------------------------------------------------------------
bool PlateLocalization::SegmentLargestObject(IplImage *img)
{
	//DebugImage try1(img, "omg");

	int maxSize = 0, maxX = 0, maxY = 0;
	IplImage *tmp = cvCreateImage(cvGetSize(img), IPL_DEPTH_8U, 1);

	//	Clear tmp arr
	for (int x = 0; x < img->width; ++x)
		for (int y = 0; y < img->height; ++y)
			CV_IMAGE_ELEM(tmp, uchar, y, x) = 0;

	for (int x = 0; x < img->width; ++x)
	{
		for (int y = 0; y < img->height; ++y)
		{
			uchar &tf (CV_IMAGE_ELEM(tmp, uchar, y, x ));
			if (tf == 1)
				continue;	//	Already visited

			uchar b = CV_IMAGE_ELEM(img, uchar, y, x * 3 + 0 );
			uchar g = CV_IMAGE_ELEM(img, uchar, y, x * 3 + 1 );
			uchar r = CV_IMAGE_ELEM(img, uchar, y, x * 3 + 2 );

			if (b == 0 && g == 0 && r == 0)
				continue;

			//	BFS from current location
			int size = SegmentObject(img,tmp,x,y);	

			if (size > maxSize)
			{
				maxSize = size;
				maxX = x;
				maxY = y;
			}
		}
	}

	if (!maxSize)
	{
		cvReleaseImage(&tmp);
		return false;
	}

	//	Clear tmp arr
	for (int x = 0; x < img->width; ++x)
		for (int y = 0; y < img->height; ++y)
			CV_IMAGE_ELEM(tmp, uchar, y, x) = 0;

	//	Put 1's only on the largest chunk 
	if (SegmentObject(img,tmp,maxX, maxY) != maxSize)
	{
		cvReleaseImage(&tmp);
		return false;
	}

	//	Applying tf mask to the image
	for (int x = 0; x < img->width; ++x)
		for (int y = 0; y < img->height; ++y)
		{
			if (CV_IMAGE_ELEM(tmp, uchar, y, x) == 0)
			{
				CV_IMAGE_ELEM(img, uchar, y, x * 3 + 0 ) = 0;
				CV_IMAGE_ELEM(img, uchar, y, x * 3 + 1 ) = 0;
				CV_IMAGE_ELEM(img, uchar, y, x * 3 + 2 ) = 0;
			}
		}

		FillObject(img);
		cvReleaseImage(&tmp);
		return true;
}

//--------------------------------------------------------------------------
//	Function:		SegmentObject()
//	Description:	Segmenting an object in scr's coor of (x,y) and applying
//					1's to the appropriate place in the mask picture.
//--------------------------------------------------------------------------
int PlateLocalization::SegmentObject(const IplImage *src, IplImage *mask, int x, int y)
{
	typedef std::pair<int,int> t_cord;
	queue<t_cord> q;
	q.push(t_cord(y, x));
	int size = 0;
	//t_cord topleft(row,col), bottomright(row,col); // save bounding box
	while (!q.empty()) {
		t_cord curr (q.front()); 
		q.pop();
		uchar &b(CV_IMAGE_ELEM(src, uchar, curr.first, curr.second * 3 + 0));
		uchar &g(CV_IMAGE_ELEM(src, uchar, curr.first, curr.second * 3 + 1));
		uchar &r(CV_IMAGE_ELEM(src, uchar, curr.first, curr.second * 3 + 2));
		uchar &tf(CV_IMAGE_ELEM(mask, uchar, curr.first, curr.second));
		if (tf || (r == 0 && g == 0 && b == 0))
			continue;

		++size;
		tf = 1;

		// doing the bfs action, basically:
		if (curr.first > 0)
			q.push( t_cord(curr.first - 1, curr.second) );
		if (curr.first + 1 < src->height)
			q.push( t_cord(curr.first + 1, curr.second) );
		if (curr.second > 0)
			q.push( t_cord(curr.first, curr.second - 1) );
		if (curr.second + 1 < src->width)
			q.push( t_cord(curr.first, curr.second + 1) );
	}
	return size;
}

//--------------------------------------------------------------------------
//	Function:		FillObject()
//	Description:	Filling the only object of the image in white.
//--------------------------------------------------------------------------
void PlateLocalization::FillObject(IplImage *img)
{
	for (int x = 0; x < img->width; ++x)
	{
		int y_min = img->height - 1, y_max = 0;
		for (int y = 0; y < img->height; ++y)
		{
			uchar b = CV_IMAGE_ELEM(img, uchar, y, x * 3 + 0 );
			uchar g = CV_IMAGE_ELEM(img, uchar, y, x * 3 + 1 );
			uchar r = CV_IMAGE_ELEM(img, uchar, y, x * 3 + 2 );

			if (!r && !b && !g)
				continue;

			if (y < y_min)
				y_min = y;
			if (y > y_max)
				y_max = y;
		}

		for (int y = y_min ; y <= y_max; ++y)
		{
			CV_IMAGE_ELEM(img, uchar, y, x * 3 + 0 ) = 255;
			CV_IMAGE_ELEM(img, uchar, y, x * 3 + 1 ) = 255;
			CV_IMAGE_ELEM(img, uchar, y, x * 3 + 2 ) = 255;
		}
	}
}

//--------------------------------------------------------------------------
//	Function:		ExtractYellowFromImg()
//	Description:	Locating yellow segments in picture and erasing all
//					other objects. Returns false if no suitable yellow 
//					substance was found - no license plate in picture.
//--------------------------------------------------------------------------
bool PlateLocalization::ExtractYellowFromImg(IplImage *img)
{
#ifdef _DEBUG
	DebugImage dbug(img, "ExtractYellowFromImg");
#endif

	IplImage *src = img;

	if (!TresholdYellow(img) || !SegmentLargestObject(img))
	{
		cvReleaseImage(&img);
		return false;
	}


	return true;
}

//--------------------------------------------------------------------------
//---------															--------
//---------			Finding the LP bounding box methods				--------
//---------															--------
//--------------------------------------------------------------------------
//	Function:		FindSquare()
//	Description:	Finding the bounding "box" of the single yellow objects
//					remaining.
//--------------------------------------------------------------------------
bool PlateLocalization::FindSquare(IplImage *img, CvPoint *points)
{
#ifdef _DEBUG
	DebugImage dbug(img, "FindSquare");
#endif

	IplImage *tmpgray = cvCreateImage(cvGetSize(img), 8 ,1);
	IplImage *tmpcolor = cvCreateImage(cvGetSize(img), 8 ,3);
	cvCvtColor(img, tmpgray, CV_BGR2GRAY);
	cvCanny(tmpgray, tmpgray, 50, 200, 3);
	cvCvtColor(tmpgray, tmpcolor, CV_GRAY2BGR);

	CvMemStorage* storage = cvCreateMemStorage(0);
	CvSeq* lines = 0;
	int vertLinesFound = 0;
	int horiLinesFound = 0;
	float horiLines[4] = {0, 0, 0, 0};
	float vertLines[4] = {0, 0, 0, 0};
	int i;

	//	Do several iterations - from the most accurate line detection to the most roubost
	for (int iteration = 4; iteration >= 0 && (vertLinesFound < 2 || horiLinesFound < 2) ; --iteration){

		IplImage *tmp = cvCloneImage(tmpcolor);


		lines = cvHoughLines2( tmpgray, storage, CV_HOUGH_STANDARD, 1, CV_PI/180, 4 + iteration * 22, 0, 0 );

		for(int i = 0; i < lines->total; i++ ){
			float* line = (float*)cvGetSeqElem(lines,i);
			float rho = line[0];
			float theta = line[1];

//#ifdef _DEBUG
			//DebugImage sakdjfaks(tmp,"sdfjal");
//#endif

			CvPoint pt1, pt2;
			double a = cos(theta), b = sin(theta);
			double x0 = a*rho, y0 = b*rho;

			pt1.x = cvRound(x0 + 1000*(-b));
			pt1.y = cvRound(y0 + 1000*(a));
			pt2.x = cvRound(x0 - 1000*(-b));
			pt2.y = cvRound(y0 - 1000*(a));

			cvLine( tmp , pt1, pt2, CV_RGB(255,0,0), 3, 8 );

			if (theta < 0)
				theta += 2 * CV_PI;

			//	Find horizontal lines
			if (horiLinesFound < 2 && 
				theta > CV_PI * (0.5 - 35.0/180) && theta < CV_PI * (0.5 + 35.0/180)){// && //	88 < theta < 92 -> vertical line
					//(horiLines[0] < rho - PLATERES || horiLines[0] > rho + PLATERES)) {	

					bool lineOccured = false;

					if (horiLinesFound == 1){
						//	Check for intersection. if there is, dont take this lines

						if (CheckIntersection(rho, theta, horiLines[0], horiLines[1], tmpgray->width, tmpgray->height))
							lineOccured = true;

					}

					if (!lineOccured){
						horiLines[2 * horiLinesFound + 0] = rho;
						horiLines[2 * horiLinesFound + 1] = theta;
						horiLinesFound++;

						cvLine( tmp , pt1, pt2, CV_RGB(0,255,0), 3, 8 );
					}
			}

			//	Find vertical lines
			if (vertLinesFound < 2){
				if (theta > CV_PI * (1 - 35.0/180) || theta < CV_PI * 35.0 / 180){ //  &&	//	theta < 2 || theta > 358 -> horizontal line
					//if (vertLines[0] < rho - PLATERES || vertLines[0] > rho + PLATERES){	
					bool lineOccured = false;
					if (vertLinesFound == 1){
						//	Check for intersection. if there is, dont take this lines

						if (CheckIntersection(rho, theta, vertLines[0], vertLines[1], tmpgray->width, tmpgray->height))
							lineOccured = true;

					}

					if (!lineOccured){
						vertLines[2 * vertLinesFound + 0] = rho;
						vertLines[2 * vertLinesFound + 1] = theta;
						vertLinesFound++;

						cvLine( tmp , pt1, pt2, CV_RGB(0,255,0), 3, 8 );
					}	
					//	}
				}
			}

			//	Found 4 lines. Exit.
			if (vertLinesFound == 2 && horiLinesFound == 2)
				break;
		}

		//DebugImage sadkjfh(tmp,"sdfaefa");
	}

	//	Draw found lines
	if (vertLinesFound == 2 && horiLinesFound == 2){

		float drawlines[8];
		CvPoint linesPoints[8];

		for (int i = 0; i < 8; i++){
			if (i < 4)
				drawlines[i] = vertLines[i];
			else 
				drawlines[i] = horiLines[i - 4];
		}

		for (int i = 0; i < 4; i++){
			//DebugImage forloop(tmpcolor, "tmpcolor");
			float rho = drawlines[i * 2 + 0];
			float theta = drawlines[i * 2 + 1];

			//CvPoint pt1, pt2;
			double a = cos(theta), b = sin(theta);
			double x0 = a*rho, y0 = b*rho;

			linesPoints[i * 2 + 0].x = cvRound(x0 + 1000*(-b));
			linesPoints[i * 2 + 0].y = cvRound(y0 + 1000*(a));
			linesPoints[i * 2 + 1].x = cvRound(x0 - 1000*(-b));
			linesPoints[i * 2 + 1].y = cvRound(y0 - 1000*(a));

			cvLine( tmpcolor , linesPoints[i * 2 + 0], linesPoints[i * 2 + 1], CV_RGB(255,0,0), 3, 8 );
		}


		//	Find 4 intersection points
		for (int i = 0; i < 2; i++)
		{
			//	Line1 ( vertical )
			CvPoint v0 = linesPoints[i * 2 + 0];
			CvPoint v1 = linesPoints[i * 2 + 1];

			for (int j = 0; j < 2; j++){
				CvPoint intersect;

				//	Line2 (horizontal)
				CvPoint h0 = linesPoints[j * 2 + 0 + 4];
				CvPoint h1 = linesPoints[j * 2 + 1 + 4];


				if (!FindIntersection(v0,v1,h0,h1,&intersect)){
					cvReleaseImage(&tmpgray);
					cvReleaseImage(&tmpcolor);
					return false;
				}

				points[i * 2 + j] = intersect;
			}
		}

		//	If the object is not wider then it's height, it's probably not a LP
		CvSize bb = BoundingBox(points,NULL);
		if (bb.height * 2 > bb.width){
			cvReleaseImage(&tmpgray);
			cvReleaseImage(&tmpcolor);
			return false;
		}

		cvCopyImage(tmpcolor, img);

		return true;
	}
	else {
		cvReleaseImage(&tmpgray);
		cvReleaseImage(&tmpcolor);
		return false;
	}
}

bool PlateLocalization::CheckIntersection(float rho0, float theta0, float rho1, float theta1, int width, int height){

	if (rho0 == rho1 && theta0 == theta1)
		return true;

	CvPoint v0, v1, v2, v3;
	double a, b, x0, y0;
	a = cos(theta0); 
	b = sin(theta0);
	x0 = a*rho0; 
	y0 = b*rho0;
	v0.x = cvRound(x0 + 1000*(-b));
	v0.y = cvRound(y0 + 1000*(a));
	v1.x = cvRound(x0 - 1000*(-b));
	v1.y = cvRound(y0 - 1000*(a));

	a = cos(theta1); 
	b = sin(theta1);
	x0 = a*rho1; 
	y0 = b*rho1;
	v2.x = cvRound(x0 + 1000*(-b));
	v2.y = cvRound(y0 + 1000*(a));
	v3.x = cvRound(x0 - 1000*(-b));
	v3.y = cvRound(y0 - 1000*(a));

	CvPoint inter;
	if (FindIntersection(v0,v1,v2,v3,&inter)){
		if (inter.x < 0 || inter.y < 0 || inter.x > width || inter.y > height)
			return false;
		else
			return true;
	}
}

bool PlateLocalization::FindIntersection(CvPoint v0, CvPoint v1, CvPoint h0, CvPoint h1, CvPoint *inter){

	int a1, b1, c1;
	a1 = v1.y - v0.y;
	b1 = v0.x - v1.x;
	c1 = v1.x * v0.y - v0.x * v1.y;

	int a2, b2, c2;
	a2 = h1.y - h0.y;
	b2 = h0.x - h1.x;
	c2 = h1.x * h0.y - h0.x * h1.y;

	int denom = a1*b2 - a2*b1;	//	if 0 lines are parallel - error
	if (denom == 0){
		return false;
	}
	inter->x = (((float)b1/denom)*c2 - ((float)b2/denom)*c1);
	inter->y = (((float)a2/denom)*c1 - ((float)a1/denom)*c2);

	return true;
}

CvSize PlateLocalization::BoundingBox(CvPoint *points, CvPoint *pointsOrderedTLBLTRBR){
	CvSize size;
	int maxX = points[0].x, minX = points[0].x, maxY = points[0].y, minY = points[0].y;

	if (pointsOrderedTLBLTRBR != NULL){
		for (int i = 0; i < 4; i++)
			pointsOrderedTLBLTRBR[i] = points[i];

		//	Bubble sort by x
		for (int i = 0; i < 3 ; i++){
			for (int j = 0; j < 3 - i ; j++){
				if (pointsOrderedTLBLTRBR[j].x > pointsOrderedTLBLTRBR[j+1].x){
					CvPoint tmp = pointsOrderedTLBLTRBR[j];
					pointsOrderedTLBLTRBR[j] = pointsOrderedTLBLTRBR[j+1];
					pointsOrderedTLBLTRBR[j+1] = tmp;
				}
			}
		}

		//	Set in the pointsOrderedTLBLTRBR array correctly
		if (pointsOrderedTLBLTRBR[0].y > pointsOrderedTLBLTRBR[1].y){
			CvPoint tmp = pointsOrderedTLBLTRBR[0];
			pointsOrderedTLBLTRBR[0] = pointsOrderedTLBLTRBR[1];
			pointsOrderedTLBLTRBR[1] = tmp;
		}

		if (pointsOrderedTLBLTRBR[2].y > pointsOrderedTLBLTRBR[3].y){
			CvPoint tmp = pointsOrderedTLBLTRBR[2];
			pointsOrderedTLBLTRBR[2] = pointsOrderedTLBLTRBR[3];
			pointsOrderedTLBLTRBR[3] = tmp;
		}
	}

	for (int i = 0 ; i < 4 ; i++){
		if (points[i].x < minX)
			minX = points[i].x;
		if (points[i].x > maxX)
			maxX = points[i].x;
		if (points[i].y < minY)
			minY = points[i].y;
		if (points[i].y > maxY)
			maxY = points[i].y;


	}
	size.height = maxY - minY + 1;
	size.width = maxX - minX + 1;
	return size;
}

bool PlateLocalization::TrimPicture(IplImage *img, IplImage **trimmed, CvPoint *points){

	CvPoint pointsOrderedTLBLTRBR[4];
	CvSize BB = BoundingBox(points, pointsOrderedTLBLTRBR);
	*trimmed = cvCreateImage(BB, 8, 3);

	for (int y = 0; y < (*trimmed)->height; y++) {
		for (int x = 0; x < (*trimmed)->width; x++) {
			CV_IMAGE_ELEM((*trimmed), uchar, y, x * 3 + 0 )=255;
			CV_IMAGE_ELEM((*trimmed), uchar, y, x * 3 + 1 )=255;
			CV_IMAGE_ELEM((*trimmed), uchar, y, x * 3 + 2 )=255;
		}
	}

	/*for (int i = 0; i < 4; i++)
	{
	cvDrawCircle(img, pointsOrderedTLBLTRBR[i], 3, CV_RGB(255,0,0), 2);
	}*/

#ifdef _DEBUG
	DebugImage sdfkjasdf(*trimmed, "trimmed");
#endif

	CvPoint topLeft = pointsOrderedTLBLTRBR[0];
	CvPoint botLeft = pointsOrderedTLBLTRBR[1];
	CvPoint topRight = pointsOrderedTLBLTRBR[2];
	CvPoint botRight = pointsOrderedTLBLTRBR[3];

	CvPoint leftAnchor = topLeft;
	CvPoint rightAnchor = topRight;

	float leftcollen = sqrt((float)(topLeft.x - botLeft.x)*(topLeft.x - botLeft.x) + (topLeft.y - botLeft.y)*(topLeft.y - botLeft.y));
	float rightcollen = sqrt((float)(topRight.x - botRight.x)*(topRight.x - botRight.x) + (topRight.y - botRight.y)*(topRight.y - botRight.y));
	float leftcolstep = leftcollen / (*trimmed)->height;
	float rightcolstep = rightcollen / (*trimmed)->height;
	float leftcolstepfromcollen = leftcolstep / leftcollen;
	float rightcolstepfromcollen = rightcolstep / rightcollen;

	float leftcolStepX = botLeft.x - topLeft.x;
	float leftcolStepY = botLeft.y - topLeft.y;
	float rightcolStepX = botRight.x - topRight.x;
	float rightcolStepY = botRight.y - topRight.y;

	CvPoint left, right;


	for (int y = 0; y < (*trimmed)->height; y++) {



		//	Setting the right and left pos
		left.x = leftAnchor.x + leftcolstepfromcollen * y * leftcolStepX;
		left.y = leftAnchor.y + leftcolstepfromcollen * y * leftcolStepY;
		right.x = rightAnchor.x + rightcolstepfromcollen * y * rightcolStepX;
		right.y = rightAnchor.y + rightcolstepfromcollen * y * rightcolStepY;

		float linelen = sqrt((float)(right.x - left.x)*(right.x - left.x) + (right.y - left.y)*(right.y - left.y));
		float linestep = linelen / (*trimmed)->width;
		float linestepfromlinelen = linestep / linelen;

		float stepX = (right.x - left.x);
		float stepY = (right.y - left.y);

		for (int x = 0; x < (*trimmed)->width; x++) {
			//DebugImage tjnosd(*trimmed, "sdaf");
			//DebugImage sdkjfak(img, "bla");


			int curr_x = left.x + linestepfromlinelen * x * stepX;
			int curr_y = left.y + linestepfromlinelen * x * stepY;

			int avgr, avgg, avgb;
			avgr = (CV_IMAGE_ELEM(img, uchar, curr_y, curr_x * 3 + 2 ) + CV_IMAGE_ELEM(img, uchar, curr_y + 1, curr_x * 3 + 2 ) +
				CV_IMAGE_ELEM(img, uchar, curr_y, (curr_x + 1) * 3 + 2 ) + CV_IMAGE_ELEM(img, uchar, curr_y + 1, (curr_x + 1) * 3 + 2 )) / 4;
			avgg = (CV_IMAGE_ELEM(img, uchar, curr_y, curr_x * 3 + 1 ) + CV_IMAGE_ELEM(img, uchar, curr_y + 1, curr_x * 3 + 1 ) +
				CV_IMAGE_ELEM(img, uchar, curr_y, (curr_x + 1) * 3 + 1 ) + CV_IMAGE_ELEM(img, uchar, curr_y + 1, (curr_x + 1) * 3 + 1 )) / 4;
			avgb = (CV_IMAGE_ELEM(img, uchar, curr_y, curr_x * 3 + 0 ) + CV_IMAGE_ELEM(img, uchar, curr_y + 1, curr_x * 3 + 0 ) +
				CV_IMAGE_ELEM(img, uchar, curr_y, (curr_x + 1) * 3 + 0 ) + CV_IMAGE_ELEM(img, uchar, curr_y + 1, (curr_x + 1) * 3 + 0 )) / 4;


			CV_IMAGE_ELEM(*trimmed, uchar, y, x * 3 + 0 ) = avgb;
			CV_IMAGE_ELEM(*trimmed, uchar, y, x * 3 + 1 ) = avgg;
			CV_IMAGE_ELEM(*trimmed, uchar, y, x * 3 + 2 ) = avgr;

			//CV_IMAGE_ELEM(img, uchar, curr_y, curr_x * 3 + 0 )=0;
			//CV_IMAGE_ELEM(img, uchar, curr_y, curr_x * 3 + 1 )=0;
			//CV_IMAGE_ELEM(img, uchar, curr_y, curr_x * 3 + 2 )=0;
		}
	}

	return true;
}

